#!/usr/bin/env python3
# virtual_device_com4.py
# 虚拟串口设备（用于与后端联调），使用真实串口（如 COM4）
# Usage:
#   python virtual_device_com4.py --port COM4 --baud 115200 --debug
#
# 功能：
# - 模拟云台电机自定义协议（帧头 0x3E / 应答 0x3C）：支持命令 0x2F/0x21/0x57/0x55/0x40/0x50
# - 模拟 Modbus RTU 设备（支持 FC=0x01/0x02/0x03/0x04/0x06/0x10）
#   - 传感器地址 3 (50mm) / 4 (100mm) 对 FC=03 返回 4 字节 float（小端）
#   - 数字量模块 (默认从机地址 6) 对 FC=04（输入寄存器）返回单通道 0/1（为光电开关）
#   - 支持写寄存器 0x6002 (急停/触发)，以及 PR 寄存器 0x6200..0x6205 的写入与触发响应
# - 提供交互命令行，可在运行时调整角度、目标、传感器值、光电开关状态等

import serial
import threading
import time
import struct
import random
import argparse
import signal
import logging
from datetime import datetime

# CRC helpers
def crc16_modbus_bytes(data: bytes) -> bytes:
    crc = 0xFFFF
    for b in data:
        crc ^= b
        for _ in range(8):
            if crc & 1:
                crc >>= 1
                crc ^= 0xA001
            else:
                crc >>= 1
    return struct.pack('<H', crc)

def crc16_modbus_int(data: bytes) -> int:
    return struct.unpack('<H', crc16_modbus_bytes(data))[0]

class VirtualSerialDevice:
    def __init__(self, port="COM4", baudrate=115200, debug=False,
                 init_angle=0.0, init_50=25.0, init_100=60.0, photo_addr=6):
        self.port = port
        self.baudrate = baudrate
        self.debug = debug
        self.ser = None
        self.running = False
        self.thread = None
        self.lock = threading.Lock()

        # 模拟状态
        self.motor_angle = float(init_angle)
        self.motor_target = float(init_angle)
        self.motor_running = False
        self.packet_seq = 0

        self.sensor_50_distance = float(init_50)
        self.sensor_100_distance = float(init_100)

        # stepper / PR registers state (store last written values)
        self.pr_regs = {}  # keys like 0x6200..0x620F -> 16-bit values
        self.control_regs = {}  # e.g., 0x6002 etc.

        # photoelectric switches (digital inputs)
        self.photo_switches = {
            "positive_limit": False,
            "origin": False,
            "negative_limit": False
        }
        self.photo_modbus_addr = int(photo_addr)

        self._rx_buffer = bytearray()

        # logging
        self.logger = logging.getLogger("VirtualSerialDevice")
        self.logger.setLevel(logging.DEBUG if self.debug else logging.INFO)
        ch = logging.StreamHandler()
        ch.setFormatter(logging.Formatter("[%(asctime)s] %(levelname)s: %(message)s", "%H:%M:%S"))
        self.logger.handlers = [ch]

    def _update_motor_angle(self):
        if self.motor_running:
            diff = (self.motor_target - self.motor_angle + 540) % 360 - 180
            if abs(diff) > 0.15:
                step = 0.4 if diff > 0 else -0.4
                self.motor_angle = (self.motor_angle + step) % 360
            else:
                self.motor_angle = self.motor_target % 360
                self.motor_running = False
                self.logger.info(f"Motor reached target angle: {self.motor_angle:.2f}°")

    def _process_rx_buffer(self):
        responses = []
        buf = self._rx_buffer
        ptr = 0
        while ptr < len(buf):
            remaining = buf[ptr:]
            if remaining[0] == 0x3E:
                if len(remaining) < 5:
                    break
                data_len = remaining[4]
                frame_len = 5 + data_len + 2
                if len(remaining) < frame_len:
                    break
                frame = bytes(remaining[:frame_len])
                payload = frame[:-2]
                recv_crc = struct.unpack('<H', frame[-2:])[0]
                calc_crc = crc16_modbus_int(payload)
                if recv_crc != calc_crc:
                    self.logger.warning("Motor frame CRC mismatch, discarding first byte")
                    ptr += 1
                    continue
                resp = self._handle_motor_frame(frame)
                if resp:
                    responses.append(resp)
                ptr += frame_len
            else:
                # 尝试解析 Modbus 请求
                if len(remaining) < 8:
                    break
                # 传入 remaining 以便 handler 在需要时读取完整帧
                result = self._handle_modbus_request(bytes(remaining[:8]), remaining)
                # handler 返回 (resp_bytes, consumed_len) 或 (None, consumed_len)
                if not isinstance(result, tuple):
                    # 为兼容旧实现（不太可能），按 8 字节处理
                    resp = result
                    consumed = 8
                else:
                    resp, consumed = result
                # 如果 handler 要求消费 0 或 1 字节以跳过错误内容，处理并继续
                if consumed <= 0:
                    consumed = 1
                # 移动指针并记录响应
                if resp:
                    responses.append(resp)
                ptr += consumed
        if ptr > 0:
            self._rx_buffer = bytearray(buf[ptr:])
        return responses

    def _handle_motor_frame(self, frame: bytes) -> bytes:
        # frame: 0x3E seq id cmd len data... crc
        seq = frame[1]
        motor_id = frame[2]
        cmd = frame[3]
        data_len = frame[4]
        data_bytes = frame[5:5+data_len] if data_len > 0 else b''

        self.logger.debug(f"Motor RX: seq={seq} id={motor_id} cmd=0x{cmd:02X} len={data_len} data={data_bytes.hex()}")

        resp_header = bytearray([0x3C, seq, motor_id, cmd, 0x08])
        resp_data = b''

        if cmd == 0x2F:  # read angle
            single_turn_raw = int(self.motor_angle * (16384.0 / 360.0)) & 0x3FFF
            multi_turn_raw = 0
            resp_data = struct.pack('<H', single_turn_raw) + struct.pack('<I', multi_turn_raw) + b'\x00\x00'
        elif cmd == 0x21:  # set origin
            self.motor_angle = 0.0
            self.motor_target = 0.0
            self.motor_running = False
            resp_data = b'\x00\x00\x00\x00\x00\x00\x00\x01'
            self.logger.info("Motor set origin (cmd 0x21)")
        elif cmd == 0x57:  # set speed
            if len(data_bytes) >= 3 and data_bytes[0] == 0x01:
                speed = struct.unpack('<H', data_bytes[1:3])[0]
                self.logger.info(f"Motor set speed: {speed}")
            resp_data = b'\x00\x00\x00\x00\x00\x00\x00\x01'
        elif cmd == 0x55:  # set target position (little-endian 4 bytes)
            if len(data_bytes) >= 4:
                target_counts = struct.unpack('<I', data_bytes[:4])[0]
                # convert counts to degrees (as backend)
                self.motor_target = (target_counts * (360.0 / 16384.0)) % 360.0
                self.motor_running = True
                self.logger.info(f"Motor target set: {self.motor_target:.2f}° (counts={target_counts})")
            resp_data = b'\x00\x00\x00\x00\x00\x00\x00\x01'
        elif cmd == 0x40:  # status
            voltage = int(max(0, min(255, (24.0 + random.uniform(-1,1)) / 0.2)))
            current = int(max(0, min(255, (1.2 + random.uniform(-0.3,0.3)) / 0.03)))
            temp = int(max(0, min(255, (45.0 + random.uniform(-5,5)) / 0.4)))
            fault = 0x00
            run_mode = 0x03
            resp_data = bytes([voltage, current, temp, fault, run_mode, 0x00, 0x00, 0x00])
        elif cmd == 0x50:  # close motor
            self.motor_running = False
            resp_data = b'\x00\x00\x00\x00\x00\x00\x00\x01'
            self.logger.info("Motor close (cmd 0x50)")
        else:
            self.logger.debug(f"Unknown motor cmd 0x{cmd:02X}, replying OK")
            resp_data = b'\x00\x00\x00\x00\x00\x00\x00\x01'

        resp = bytes(resp_header) + resp_data
        resp += crc16_modbus_bytes(resp)
        self.logger.debug(f"Motor TX: {resp.hex()}")
        return resp

    def _handle_modbus_request(self, frame8: bytes, remaining: bytes):
        # Parse header
        addr = frame8[0]
        func = frame8[1]
        reg_hi = frame8[2]
        reg_lo = frame8[3]
        reg_addr = (reg_hi << 8) | reg_lo
        qty_hi = frame8[4]
        qty_lo = frame8[5]
        qty = (qty_hi << 8) | qty_lo

        consumed_len = 8  # 默认消费长度
        self.logger.debug(f"Modbus RX: addr={addr} func=0x{func:02X} reg=0x{reg_addr:04X} qty={qty}")

        # READ (FC=03/04)
        if func in (0x03, 0x04):
            if addr == self.photo_modbus_addr and func == 0x04:
                ch_index = reg_addr
                val = 0
                if ch_index == 0:
                    val = 1 if self.photo_switches["positive_limit"] else 0
                elif ch_index == 1:
                    val = 1 if self.photo_switches["origin"] else 0
                elif ch_index == 2:
                    val = 1 if self.photo_switches["negative_limit"] else 0
                hi = (val >> 8) & 0xFF
                lo = val & 0xFF
                resp = bytes([addr, func, 2, hi, lo]) + crc16_modbus_bytes(bytes([addr, func, 2, hi, lo]))
                self.logger.debug(f"Photo module reply: {resp.hex()}")
                return resp, consumed_len

            if addr == 3 and func == 0x03:
                distance = round(self.sensor_50_distance + random.uniform(-0.2,0.2), 5)
                data_bytes = struct.pack('<f', float(distance))
                resp = bytes([addr, func, len(data_bytes)]) + data_bytes + crc16_modbus_bytes(bytes([addr, func, len(data_bytes)]) + data_bytes)
                self.logger.debug(f"Sensor50 reply: {distance} -> {resp.hex()}")
                return resp, consumed_len
            if addr == 4 and func == 0x03:
                distance = round(self.sensor_100_distance + random.uniform(-0.5,0.5), 5)
                data_bytes = struct.pack('<f', float(distance))
                resp = bytes([addr, func, len(data_bytes)]) + data_bytes + crc16_modbus_bytes(bytes([addr, func, len(data_bytes)]) + data_bytes)
                self.logger.debug(f"Sensor100 reply: {distance} -> {resp.hex()}")
                return resp, consumed_len

            byte_count = qty * 2
            data_bytes = bytes([0] * byte_count)
            resp = bytes([addr, func, byte_count]) + data_bytes + crc16_modbus_bytes(bytes([addr, func, byte_count]) + data_bytes)
            self.logger.debug(f"Modbus generic reply -> {resp.hex()}")
            return resp, consumed_len

        # WRITE SINGLE REGISTER (FC=06)
        if func == 0x06:
            val_hi = frame8[4]
            val_lo = frame8[5]
            value16 = (val_hi << 8) | val_lo
            if reg_addr == 0x6002:
                self.control_regs[0x6002] = value16
                if value16 == 0x0010:
                    self.logger.info("Received trigger PR run (0x6002=0x0010)")
                elif value16 == 0x0040:
                    self.logger.info("Received emergency stop (0x6002=0x0040)")
            elif 0x6200 <= reg_addr <= 0x62FF:
                self.pr_regs[reg_addr] = value16
                self.logger.debug(f"PR reg write: 0x{reg_addr:04X} = 0x{value16:04X}")
            # echo request as response (Modbus)
            # Need to recalc CRC for echoed frame (frame8 includes CRC from request); echo must be same bytes as request
            resp = frame8
            return resp, consumed_len

        # WRITE MULTIPLE REGISTERS (FC=0x10)
        if func == 0x10:
            # need to ensure full frame present in remaining
            if len(remaining) < 7:
                # not enough data, request parser to consume minimal bytes to avoid deadlock
                return b'', 1
            bytecount = remaining[6]
            full_len = 7 + bytecount + 2
            if len(remaining) < full_len:
                # incomplete, wait for more bytes; do not consume (but to avoid infinite loop we consume 1)
                return b'', 1
            frame_full = bytes(remaining[:full_len])
            recv_crc = struct.unpack('<H', frame_full[-2:])[0]
            calc_crc = crc16_modbus_int(frame_full[:-2])
            if recv_crc != calc_crc:
                self.logger.warning("Modbus FC10 CRC mismatch, discarding first byte")
                return b'', 1
            start_reg = (frame_full[2] << 8) | frame_full[3]
            qty_regs = (frame_full[4] << 8) | frame_full[5]
            data_bytes = frame_full[7:7+bytecount]
            if start_reg >= 0x6200 and start_reg <= 0x62FF:
                for i in range(qty_regs):
                    hi = data_bytes[2*i]
                    lo = data_bytes[2*i+1]
                    val = (hi << 8) | lo
                    self.pr_regs[start_reg + i] = val
                    self.logger.debug(f"PR reg set 0x{start_reg+i:04X} = 0x{val:04X}")
            if start_reg in (0x6000, 0x6001, 0x6002):
                for i in range(qty_regs):
                    hi = data_bytes[2*i]
                    lo = data_bytes[2*i+1]
                    val = (hi << 8) | lo
                    self.control_regs[start_reg + i] = val
                    self.logger.debug(f"Control reg set 0x{start_reg+i:04X} = 0x{val:04X}")
            ack = bytes([addr, func, (start_reg >> 8) & 0xFF, start_reg & 0xFF, (qty_regs >> 8) & 0xFF, qty_regs & 0xFF])
            ack += crc16_modbus_bytes(ack)
            self.logger.debug(f"Modbus FC10 ack -> {ack.hex()}")
            return ack, full_len

        # READ COILS / DISCRETE INPUTS (FC=01/02)
        if func in (0x01, 0x02):
            bits = 0
            bits |= int(self.photo_switches["positive_limit"]) << 0
            bits |= int(self.photo_switches["origin"]) << 1
            bits |= int(self.photo_switches["negative_limit"]) << 2
            resp = bytes([addr, func, 1, bits & 0xFF]) + crc16_modbus_bytes(bytes([addr, func, 1, bits & 0xFF]))
            self.logger.debug(f"Modbus coils reply -> bits={bits:08b} resp={resp.hex()}")
            return resp, consumed_len

        # fallback
        self.logger.debug(f"No handler for Modbus func 0x{func:02X}")
        return b'', consumed_len

    def _ser_write(self, data: bytes):
        if not self.ser or not self.ser.is_open:
            self.logger.error("Attempt to write but serial is not open")
            return
        with self.lock:
            self.ser.write(data)
            self.ser.flush()
            if self.debug:
                self.logger.debug(f"TX: {data.hex()}")

    def _read_write_loop(self):
        self.logger.info("Virtual device loop started")
        try:
            while self.running:
                self._update_motor_angle()
                try:
                    in_wait = self.ser.in_waiting
                except Exception:
                    in_wait = 0
                if in_wait > 0:
                    new = self.ser.read(min(in_wait, 1024))
                    if new:
                        if self.debug:
                            self.logger.debug(f"RX(raw): {new.hex()}")
                        self._rx_buffer += new
                        responses = self._process_rx_buffer()
                        for r in responses:
                            if r:
                                # Some response objects may have attribute _consumed_len; ignore for writing
                                self._ser_write(r)
                else:
                    time.sleep(0.005)
        except Exception:
            self.logger.exception("Error in read/write loop")
        finally:
            self.logger.info("Virtual device loop exiting")

    def start(self, blocking=True):
        try:
            self.logger.info(f"Opening serial port {self.port} @ {self.baudrate}")
            self.ser = serial.Serial(
                port=self.port,
                baudrate=self.baudrate,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
                bytesize=serial.EIGHTBITS,
                timeout=0.05
            )
        except Exception as e:
            self.logger.exception(f"Open serial failed: {e}")
            return
        self.running = True
        self.thread = threading.Thread(target=self._read_write_loop, daemon=True)
        self.thread.start()
        if blocking:
            try:
                while self.running:
                    time.sleep(0.5)
            except KeyboardInterrupt:
                self.stop()

    def stop(self):
        self.logger.info("Stopping virtual device...")
        self.running = False
        if self.thread and self.thread.is_alive():
            self.thread.join(timeout=1.0)
        try:
            if self.ser and self.ser.is_open:
                self.ser.close()
        except Exception:
            pass
        self.logger.info("Virtual device stopped")

    def set_photo_switch(self, name: str, state: bool):
        if name in self.photo_switches:
            self.photo_switches[name] = bool(state)
            self.logger.info(f"Photo switch {name} set to {state}")
        else:
            self.logger.warning(f"Unknown photo switch {name}")

    def repl_commands(self):
        self.logger.info("Commands: angle N | target N | sensor50 N | sensor100 N | set pos|origin|neg 0/1 | quit")
        while self.running:
            try:
                cmd = input().strip()
                if not cmd:
                    continue
                parts = cmd.split()
                if parts[0] in ('quit', 'q'):
                    self.stop()
                    break
                if parts[0] == 'angle' and len(parts) >= 2:
                    v = float(parts[1])
                    self.motor_angle = v % 360
                    self.logger.info(f"Angle set to {self.motor_angle}")
                elif parts[0] == 'target' and len(parts) >= 2:
                    v = float(parts[1])
                    self.motor_target = v % 360
                    self.motor_running = True
                    self.logger.info(f"Target set to {self.motor_target}")
                elif parts[0] == 'sensor50' and len(parts) >= 2:
                    self.sensor_50_distance = float(parts[1])
                    self.logger.info(f"Sensor50 set to {self.sensor_50_distance}")
                elif parts[0] == 'sensor100' and len(parts) >= 2:
                    self.sensor_100_distance = float(parts[1])
                    self.logger.info(f"Sensor100 set to {self.sensor_100_distance}")
                elif parts[0] == 'set' and len(parts) >= 3:
                    name = parts[1]
                    val = parts[2] in ('1', 'on', 'true', 'True')
                    if name in ('pos', 'positive_limit'):
                        self.set_photo_switch('positive_limit', val)
                    elif name in ('origin',):
                        self.set_photo_switch('origin', val)
                    elif name in ('neg', 'negative_limit'):
                        self.set_photo_switch('negative_limit', val)
                    else:
                        self.logger.warning("Unknown set target")
                else:
                    self.logger.info("Unknown command")
            except Exception:
                self.logger.exception("REPL error")
                break

def main():
    p = argparse.ArgumentParser(description="Virtual serial device simulator (real COM port)")
    p.add_argument("--port", "-p", default="COM4", help="Serial port (default COM4)")
    p.add_argument("--baud", "-b", default=115200, type=int)
    p.add_argument("--debug", action="store_true")
    p.add_argument("--angle", type=float, default=0.0)
    p.add_argument("--s50", type=float, default=25.0)
    p.add_argument("--s100", type=float, default=60.0)
    p.add_argument("--photo_addr", type=int, default=6)
    args = p.parse_args()

    dev = VirtualSerialDevice(port=args.port, baudrate=args.baud, debug=args.debug,
                              init_angle=args.angle, init_50=args.s50, init_100=args.s100, photo_addr=args.photo_addr)

    def _sigterm(sig, frame):
        dev.logger.info("Signal received, stopping...")
        dev.stop()
    signal.signal(signal.SIGINT, _sigterm)
    signal.signal(signal.SIGTERM, _sigterm)

    start_thread = threading.Thread(target=dev.start, kwargs={"blocking": True}, daemon=True)
    start_thread.start()
    time.sleep(0.2)
    try:
        dev.repl_commands()
    except KeyboardInterrupt:
        pass
    finally:
        dev.stop()

if __name__ == "__main__":
    main()